Optimality and limit behavior of the ML estimator for Multi-Robot Localization via GPS and Relative Measurements

This work addresses the problem of distributed multi-agent localization in presence of heterogeneous measurements and wireless communication. The proposed algorithm integrates low precision global sensors, like GPS and compasses, with more precise relative position (i.e., range plus bearing) sensors. Global sensors are used to reconstruct the absolute position and orientation, while relative sensors are used to retrieve the shape of the formation. A fast distributed and asynchronous linear least-squares algorithm is proposed to solve an approximated version of the non-linear Maximum Likelihood problem. The algorithm is provably shown to be robust to communication losses and random delays. The use of ACK-less broadcast-based communication protocols ensures an efficient and easy implementation in real world scenarios. If the relative measurement errors are sufficiently small, we show that the algorithm attains a solution which is very close to the maximum likelihood solution. The theoretical findings and the algorithm performances are extensively tested by means of Monte-Carlo simulations. I. MATHEMATICAL PRELIMINARIES Resorting to standard graph theory, the estimation problem can be naturally associated with an undirected measurement graph G = (V;E) where V∈ {1, . . . ,N} represents the nodes and E⊂ V×V contains the unordered pairs of nodes {i, j} which are connected to and measure each other. We denote with Ni ⊆V the set { j | {i, j} ∈ E}, i.e. the neighboring set of node i. An undirected graph G is said to be connected if for any pair of vertices {i, j} a path exists, connecting i to j. In the problem at hand, we consider a communication graph among the nodes which coincides with the measurements graph G. Moreover, broadcast and asynchronous communications are assumed among the nodes. We denote with | · | the modulus of a scalar. Assuming M to be the cardinality of E, the incidence matrix A ∈ RM×N of G is defined as A = [aei], where aei = {1,−1,0}, if edge e is incident on node i and directed away from it, is incident on node i and directed toward it, or is not incident on node i, respectively. We denote with the symbol ‖ · ‖ the vector 2-norm and with [·]T the transpose operator. The symbol represents the Hadamard product. Given a vector v ∈ R2, the function atan2(·) :R2→ [0,2π] returns its angle, i.e., v= ‖v‖e j atan2(v). Given a matrix v ∈ R2×n, with vctr. we denote the vector centroid, i.e., vcrt. = 1 n ∑ n i=1 vi, where vi is the ith row of the matrix. The symbol σx denotes the standard deviation †A. Franchi is with LAAS-CNRS, 7 Avenue du Colonel Roche, 31077 Toulouse CEDEX 4, France. [email protected] ∗R. Carli, A. Carron, L. Schenato and M. Todescato are with the Department of Information Engineering (DEI) of the University of Padova, Via Gradenigo 6/B, 35100 Padova, Italy. [ carlirug | carronan | schenato | todescat ]@dei.unipd.it of the generic measurement x. The operator E[·] denotes the expected value, while proj(·) : R → R2 denotes the function proj(θ) = [ cosθ sinθ ]T . Finally, I denotes the identity matrix of suitable dimensions. II. PROBLEM FORMULATION Consider the problem of estimating the 2D positions, expressed in a common reference frame, of N nodes of a sensor network. Each node of the network is endowed with a set of sensors that provide both relative and absolute measurements. In the following, firstly, we introduce the statistical models exploited for each type of measurements. Secondly, we formulate the non linear Maximum-Likelihood estimation problem. Thirdly, we introduce an suitable linear and convex reformulation. A. Measurement Model We assume that the N nodes are provided with a GPS module, a compass, a relative range sensor, and a relative bearing sensor. We denote with pi = (xi,yi), i ∈ V, the 2D position of node i in a common inertial frame, and with θi its orientation with respect to the inertial North axis, which in the following we assume to coincide with the x-axis. Each sensor is described by the following statistical model: 1) The GPS measurement pGPS i = (x GPS i ,y GPS i ) represents a noisy measurement of pi = (xi,yi). We assume a normal distribution of the GPS measurements, that is pGPS i ∼ N (pi,σ pI). 2) The compass provides a noisy measurement θC i of θi. This is modelled according to an angular Gaussian distribution (see, e.g., [1]) which approximates the Langevin distribution [2]. This reads as proj(θC i )∼ N ( proj(θi),σ θ I ) . 3) The range sensor returns a noisy measurement ri j of the distance between nodes i and j, which is modelled according to a normal distribution, that is ri j ∼ N (‖pi− p j‖,σ r ). 4) The bearing sensor returns a noisy measurement δi j of the bearing angle of the node j in the local frame of node i. For δi j we adopt an angular Gaussian distribution model which reads as proj(δi j)∼ N ( proj(atan2(p j− pi)−θi),σ δ I ) . Remark II.1. Observe that, in order to reduce the set-up cost, each node has access to highly noisy absolute measurements together with relative measurements that are less prone to noise than the absolute ones. In particular, the GPS sensors are usually characterized by a standard deviation σp = 2 [m] [3], [4], while the compass by a standard deviation σθ = 0.05 [rad] [5]. To retrieve information about range and bearing different methods can be used, e.g., depth-camera, laser, ultrasound. Acceptable values for the standard deviation of these measurements might be σr = 0.1 [m] and σδ = 0.03 [rad]. Due to the variability in the accuracy of the available sensors, we will test our algorithm in a sufficiently wide range of standard deviation values. For the sake of simplicity, we consider that all the nodes are endowed with a GPS module. However, a simple reformulation of the problem would still guarantee that all the results hold even if a reduced number of nodes are provided with a GPS. B. Maximum-Likelihood Estimator We assume that all the measurements are independent and their probability distributions are given in the previous section. It is possible to formulate the localization problem as a Maximum-Likelihood (ML) estimation problem [6]. Let us define the state and measurements sets, respectively, as x = {p,θ}= {pi, θi with i ∈ V} , y = { pGPS i , θ C i , rhk, δhk with i ∈ V, (h,k) ∈ E } , where p := [p1, . . . , pN ]T and θ := [θ1, . . . ,θN ]T . Then, the negative log-likelihood cost function can be written as J(x) :=− log f ( y |x ) = Jp + Jθ + Jr + Jδ + c, (1)